#include <vector>
#include "vertex.h"
#include "movingpolygon.h"

void MovingPolygon::update(double time){

	center.x = center.x + t.x;
	center.y = center.y + t.y - 0.001;
	
	angle = angle + d_angle;
	if (angle >= 360)
		angle = angle - 360;
	this->CalcRotMatrix();
}

Vertex MovingPolygon::nthVertex(int n){
	if (n >= mesh->getN())
		n -= mesh->getN();
	if (n == -1)
		n = mesh->getN() - 1;
	Vertex p = mesh->getVertices()[n];

	float x;
	x= p.x*s[0][0] + p.y*s[0][1];
	p.y = p.x*s[1][0] + p.y*s[1][1];
	p.x = x;

	p.x = p.x + center.x + t.x;
	p.y = p.y + center.y + t.y;

	return p;
}

MovingPolygon::MovingPolygon(const char* file_name):Polygon(file_name)
{}

void MovingPolygon::SetT(Vertex T){ t = T; }

void MovingPolygon::SetdAngle(float a){ d_angle = a; }

Vertex MovingPolygon::GetV(){ return t; }

float MovingPolygon::GetV_angle() { return d_angle; }

void MovingPolygon::collide(float j, Vertex n, Vertex p, Vertex cen){
	t.x = t.x + j*n.x/m;
	t.y = t.y + j*n.y/m;
	d_angle = d_angle + j*dot_product(diff(cen, p), n);
	//this->CalcRotMatrix();
}

Vertex MovingPolygon::getCenter(){
	Vertex c;
	c.x = center.x + t.x;
	c.y = center.y + t.y;
	c.z = 0;
	return c;
}


void MovingPolygon::CalcRotMatrix(){
	float a = (angle + d_angle)*3.1415/180;
	float cos_a = cos(a);
	float sin_a = sin(a);
	s[0][0] = cos_a;
	s[0][1] = -sin_a;
	s[1][0] = sin_a;
	s[1][1] = cos_a;
}